From bb7ea2c99a67c8b78e2ec2567491cd842a1a6bbf Mon Sep 17 00:00:00 2001 From: robertl Date: Thu, 17 Jul 2003 05:09:05 +0000 Subject: [PATCH] Add rest of Ron's arc distance stuff. --- gpsbabel/arcdist.c | 258 +++++++++++++++++++++++++++++++++++++++++++++ gpsbabel/testo | 10 ++ 2 files changed, 268 insertions(+) create mode 100644 gpsbabel/arcdist.c diff --git a/gpsbabel/arcdist.c b/gpsbabel/arcdist.c new file mode 100644 index 000000000..9e8971f49 --- /dev/null +++ b/gpsbabel/arcdist.c @@ -0,0 +1,258 @@ +/* + Distance from point to arc filter + + Copyright (C) 2002 Robert Lipe, robertlipe@usa.net + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111 USA + + */ +#include +#include +#include "defs.h" + +#ifndef M_PI +# define M_PI 3.14159265358979323846 +#endif + +extern queue waypt_head; + +static double pos_dist; +static char *distopt; +static char *arcfileopt; + +typedef struct { + double distance; +} extra_data; + +static +arglist_t arcdist_args[] = { + {"file", &arcfileopt, "File containing vertices of arc"}, + {"distance", &distopt, "Maximum distance from arc"}, + {0, 0, 0} +}; + +static double gcdist( double lat1, double lon1, double lat2, double lon2 ) { + return acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon1-lon2)); +} + + +static double linedist(double lat1, double lon1, + double lat2, double lon2, + double lat3, double lon3 ) { + double lat4, lon4; + + double x1,y1,z1; + double x2,y2,z2; + double x3,y3,z3; + + double xa,ya,za,la; + + double xp,yp,zp,lp; + + double dot; + + double d12, d1p, dp2; + + double dist; + + /* degrees to radians */ + lat1 *= M_PI/180.0; lon1 *= M_PI/180.0; + lat2 *= M_PI/180.0; lon2 *= M_PI/180.0; + lat3 *= M_PI/180.0; lon3 *= M_PI/180.0; + + /* polar to ECEF rectangular */ + x1 = cos(lon1)*cos(lat1); y1 = sin(lat1); z1 = sin(lon1)*cos(lat1); + x2 = cos(lon2)*cos(lat2); y2 = sin(lat2); z2 = sin(lon2)*cos(lat2); + x3 = cos(lon3)*cos(lat3); y3 = sin(lat3); z3 = sin(lon3)*cos(lat3); + + /* 'a' is the axis; the line that passes through the center of the earth + * and is perpendicular to the great circle through point 1 and point 2 + * It is computed by taking the cross product of the '1' and '2' vectors.*/ + xa = y1*z2-y2*z1; + ya = z1*x2-z2*x1; + za = x1*y2-y1*x2; + la = sqrt(xa*xa+ya*ya+za*za); + + if ( la ) { + xa /= la; + ya /= la; + za /= la; + + /* dot is the component of the length of '3' that is along the axis. + * What's left is a non-normalized vector that lies in the plane of + * 1 and 2. */ + dot = x3*xa+y3*ya+z3*za; + + xp = x3-dot*xa; + yp = y3-dot*ya; + zp = z3-dot*za; + + lp = sqrt(xp*xp+yp*yp+zp*zp); + + if ( lp ) { + xp /= lp; + yp /= lp; + zp /= lp; + + /* convert the point 'p' (the closest point to '3' on the great circle + * that passest through '1' and '2') back to lat/lon */ + lat4 = asin( yp ); + lon4 = atan2( zp/cos(lat4), xp/cos(lat4)); + + /* compute a few distances */ + d12 = gcdist(lat1,lon1,lat2,lon2); + d1p = gcdist(lat1,lon1,lat4,lon4); + dp2 = gcdist(lat4,lon4,lat2,lon2); + + if ( d12 >= d1p && d12 >= dp2 ) { + /* if 'p' is closer to both 1 and 2 than 1 is to 2, p is between. */ + dist = gcdist(lat3,lon3,lat4,lon4); + } + else { + /* otherwise, get the distance from the closest endpoint */ + if ( d1p < dp2 ) { + dist = gcdist(lat1,lon1,lat3,lon3); + } + else { + dist = gcdist(lat2,lon2,lat3,lon3); + } + } + } + else { + /* lp is 0 when 3 is 90 degrees from the great circle */ + dist = M_PI/2; + } + } + else { + /* la is 0 when 1 and 2 are either the same point or 180 degrees apart */ + d12 = gcdist(lat1,lon1,lat2,lon2); + if ( d12 < 1 ) { + /* less than a radian : same point */ + dist = gcdist(lat1,lon1,lat3,lon3); + } + else { + /* more than a radian : 180 degrees apart; there's a great circle that + * goes through all 3 points, so the distance is 0 */ + dist = 0; + } + } + return dist; +} + +#define BADVAL 999999 + +void +arcdist_process(void) +{ + queue * elem, * tmp; + waypoint * waypointp; + double dist; + waypoint ** comp; + int i, wc; + queue temp_head; + extra_data *ed; + + FILE *arcfile = fopen( arcfileopt, "r" ); + if ( arcfile ) { + double lat1, lon1, lat2, lon2; + lat1 = lon1 = lat2 = lon2 = BADVAL; + lon1 = -999999; + while ( !feof(arcfile)) { + char line[200]; + char *pound = NULL; + + fgets( line, 200, arcfile ); + + pound = strchr( line, '#' ); + if ( pound ) *pound = '\0'; + + lat2 = lon2 = BADVAL; + sscanf( line, "%lf %lf", &lat2, &lon2 ); + + if ( lat1 != BADVAL && lon1 != BADVAL && + lat2 != BADVAL && lon2 != BADVAL ) { + QUEUE_FOR_EACH(&waypt_head, elem, tmp) { + + waypointp = (waypoint *)elem; + dist = linedist(lat1, lon1, lat2, lon2, + waypointp->position.latitude.degrees, + waypointp->position.longitude.degrees ); + + /* convert radians to float point statute miles */ + dist = (((dist * 180.0 * 60.0) / M_PI) * 1.1516); + + if ( waypointp->extra_data ) { + ed = waypointp->extra_data; + } + else { + ed = xcalloc(1, sizeof(*ed)); + ed->distance = BADVAL; + } + if ( ed->distance > dist ) { + ed->distance = dist; + } + waypointp->extra_data = ed; + } + } + lat1 = lat2; + lon1 = lon2; + } + + fclose(arcfile); + } + + + QUEUE_FOR_EACH(&waypt_head, elem, tmp) { + waypoint *wp = (waypoint *) elem; + ed = wp->extra_data; + wp->extra_data = NULL; + if ( ed ) { + if (ed->distance >= pos_dist) { + waypt_del(wp); + waypt_free(wp); + continue; + } + xfree( ed ); + } + } +} + +void +arcdist_init(const char *args) { + char *fm; + + pos_dist = 0; + + if (distopt) { + pos_dist = strtod(distopt, &fm); + + if ((*fm == 'k') || (*fm == 'K')) { + /* distance is kilometers, convert to feet */ + pos_dist *= .6214; + } + } +} + +void +arcdist_deinit(void) { + /* do nothing */ +} + +filter_vecs_t arcdist_vecs = { + arcdist_init, + arcdist_process, + arcdist_deinit, + arcdist_args +}; diff --git a/gpsbabel/testo b/gpsbabel/testo index 3c2844978..c9685edde 100755 --- a/gpsbabel/testo +++ b/gpsbabel/testo @@ -316,3 +316,13 @@ ${PNAME} -i easygps -f reference/easygps.loc -o easygps -F ${TMPDIR}/ez.loc ${PNAME} -i easygps -f reference/easygps.loc -o gpx -F ${TMPDIR}/ez1.gpx ${PNAME} -i easygps -f ${TMPDIR}/ez.loc -o gpx -F ${TMPDIR}/ez2.gpx compare ${TMPDIR}/ez1.gpx ${TMPDIR}/ez2.gpx + +# +# Arc Distance filter +# +rm -f ${TMPDIR}/arcdist.txt +${PNAME} -i xmap -f reference/arcdist_input.txt \ + -x arc,file=reference/arcdist_arc.txt,distance=1 \ + -o xmap -F ${TMPDIR}/arcdist.txt +compare ${TMPDIR}/arcdist.txt reference/arcdist_output.txt + -- 2.30.2